/**
 * Author(s): xuming.Wong <xuming.Wong@gmail.com>
 */
#include "BlenderSkeletonLoadImpl.h"

#include "GEngine.h"
#include "Logger.h"
#include "MathUtil.h"
#include "BlenderLoadUtil.h"
#include "BlenderDefine.h"

namespace GNgine
{
	BlenderBone::BoneList BlenderBone::_list;
	BlenderSkeletonLoadImpl::BlenderSkeletonLoadImpl(bParse::bBlenderFile* blenderFile,Blender::Object *object,  Ogre::Skeleton *skeleton)
		: _bFile(blenderFile), _bObject(object), _skeleton(skeleton),_transform(),_bArmature(0),_bones()
	{
		assert(_skeleton);
		assert(_bObject);
		assert(_bObject->data);
		_bArmature= static_cast<Blender::bArmature *>(_bObject->data);
		_transform=Ogre::Matrix4::IDENTITY;
	}

	void BlenderSkeletonLoadImpl::load()
	{
		Ogre::Matrix4 mat=MathUtil::getFromFloat(_bObject->obmat);
		Ogre::Matrix4 inv=mat.inverse();
		
		// load current transform
		_transform=inv*mat;//must be identity?BUG?

		// create bone lists && transforms
		Blender::Bone *bone= static_cast<Blender::Bone*>(_bArmature->bonebase.first);
		while (bone){
			// only process current bones
			if (bone->parent == 0){
				buildBoneTree(bone, 0, 0);
			}
			bone= bone->next;
		}

		// no animation data assigned
		if(!_bObject->pose){
			return;
		}

		assert(_bFile);
		bParse::bMain *bMainPtr=_bFile->getMain();
		// Loop all actions and find ones that contain this skeleton's bone names
		// FIXME: It would really be nice to convert only wanted animations. but you
		// cannot tell beforehand if 1 of 100 are needed, or all 100!
		bParse::bListBasePtr* actions=bMainPtr->getAction();
		if (!actions){
			return;
		}

		for (int i=0; i<actions->size(); i++){
			Blender::bAction *act= reinterpret_cast<Blender::bAction*>(actions->at(i));
			Blender::bActionChannel *channel= static_cast<Blender::bActionChannel*>(act->chanbase.first);
			if (channel){
				/// find if the channel belongs to this armature (whats the proper way ?)
				BoneList::iterator it=_bones.find(channel->name);
				if(it!=_bones.end()){
					actionToAnimation(_bObject->pose,act);
				}
				
			}
		}

	}

	void BlenderSkeletonLoadImpl::actionToAnimation(Blender::bPose *pose, Blender::bAction *action)
	{
		assert(pose);
		assert(action);

		Blender::bPoseChannel* pchan;
		Blender::bActionChannel *achan;
		Blender::Ipo *ipo;
		Blender::IpoCurve *icu;
		Blender::BezTriple *bezt;
		Blender::Bone *bone;

		/// find the total length
		//trange[0] is frame position curve seg start 
		//trange[1] is frame position curve seg end 
		Ogre::Real trange[2]={Ogre::Real(FLT_MAX),-Ogre::Real(FLT_MAX)};
		pchan= static_cast<Blender::bPoseChannel*>(pose->chanbase.first);
		while (pchan){
			achan= findActionChannel(pchan->name, action);
			if (achan && achan->ipo){
				ipo= achan->ipo;
				icu= (Blender::IpoCurve*)ipo->curve.first;

				while (icu){
					if (icu->bezt){
						bezt= icu->bezt;
						for (int c=0; c<icu->totvert; c++, bezt++){
							if (trange[0] > bezt->vec[1][0]){//x point 
								trange[0]= bezt->vec[1][0];
							}
							if (trange[1] < bezt->vec[1][0]){
								trange[1]= bezt->vec[1][0];
							}
						}
					}
					icu= icu->next;
				}
			}
			pchan= pchan->next;
		}

		Ogre::Real fps=GNgEngine.getAnimSpeed();
		Ogre::Real ifps=1.0f/fps;
		//the curve seg last time in second
		Ogre::Real len=(trange[1]-trange[0])/fps;//trange[1]-trange[0]) is frame length
		if(len<=0){
			GNgLog.out("Warning: Skipped action %s it contains no data!", BLENDER_IDNAME(action));
			return ;
		}

		GNgLog.out("Creating animation '%s' (%i, %i) at %i frames per second",BLENDER_IDNAME(action), int(trange[0]), int(trange[1]), int(fps));

		Ogre::Animation *anim= _skeleton->createAnimation(BLENDER_IDNAME(action), len);

		int ctime= int(trange[0]);
		while (ctime < int(trange[1])){//iterpolate the curve according to fps?
			const Ogre::Real otime= (Ogre::Real(ctime) - trange[0]) * ifps;//otime: output time (in seconds)

			/// update curve points for this frame
			/// This involves re-calculating the pose matrix!
			/// @todo add IK-tree support
			/// @todo add constraint channel support
			/// @todo ipo drivers, what are they for ?

			setPoseMatrices(pose, action, Ogre::Real(ctime));//ctime in frame 

			// Apply pose keyframes
			bone= static_cast<Blender::Bone*>(_bArmature->bonebase.first);
			while (bone){
				/// this is recursive from roots
				if (bone->parent == 0){
					buildPoseTreeKeyFrame(anim, bone, NULL, otime);
				}
				bone= bone->next;
			}

			// next frame
			++ctime;
		}

		// TODO: fix key frame loop and make this obsolete
		anim->optimise();
	}

	void BlenderSkeletonLoadImpl::buildPoseTreeKeyFrame(Ogre::Animation *anim, Blender::Bone *cur, BlenderBone *parent, const Ogre::Real& time)
	{
		assert(anim);
		assert(cur);

		BoneList::iterator it=_bones.find(cur->name);
		if(it==_bones.end()){
			GNgLog.out("Warnning: missing bone for pose channel '%s'", cur->name);
			return;
		}
		BlenderBone *bone=it->second;
		Ogre::Matrix4 posemat=bone->poseMat;
		posemat=_transform*posemat;

		// store inverse for next iteration
		bone->poseInvert=posemat.inverse();
		if(bone->parent!=0){
			posemat=parent->poseInvert*posemat;
		}

		/// build key frame transform
		Ogre::Matrix4 invbone=bone->boneMat.inverse();
		Ogre::Vector3 loc=posemat.getTrans();
		posemat=invbone*posemat;
		
		Ogre::Quaternion rot=posemat.extractQuaternion().normalise();
		Ogre::Matrix4 invrot(rot.Inverse());
		posemat=invrot*posemat;
		Ogre::Vector3 scale=Ogre::Vector3(posemat[0][0],posemat[1][1], posemat[2][2]);

		/// create the ogre keyframe
		assert(bone->bone);
		Ogre::NodeAnimationTrack *nat=0;
		if(anim->hasNodeTrack(bone->getHandle())){
			nat=anim->getNodeTrack(bone->getHandle());
		}else{
			nat=anim->createNodeTrack(bone->getHandle(),bone->bone);
		}
		assert(nat);
		///Optimise Node Tracks
		if (!loc.positionEquals(bone->old_poseloc, PQ3D_EPSILON) ||
			!rot.equals(bone->old_poserot, Ogre::Radian(PQ3D_EPSILON)) ||
			!scale.positionEquals(bone->old_poseskl, PQ3D_EPSILON)){
			/// add a new time key
			Ogre::TransformKeyFrame *tkf=nat->createNodeKeyFrame(time);
			tkf->setTranslate(loc);
			tkf->setRotation(rot);
			tkf->setScale(scale);

			bone->old_poseloc= loc;
			bone->old_poserot= rot;
			bone->old_poseskl= scale;
		}

		Blender::Bone *child= static_cast<Blender::Bone*>(cur->childbase.first);
		while (child){
			buildPoseTreeKeyFrame(anim, child, bone, time);
			child= child->next;
		}
	}


	void BlenderSkeletonLoadImpl::setPoseMatrices(Blender::bPose *pose, Blender::bAction *action,const Ogre::Real& time)
	{
		assert(pose);
		assert(action);

		Blender::bPoseChannel *pchan;
		Blender::bActionChannel *achan;
		Blender::Ipo *ipo;
		Blender::IpoCurve *icu;

		pchan= (Blender::bPoseChannel*)pose->chanbase.first;
		while (pchan){
			if (!pchan->bone){
				/// skip this channel
				pchan= pchan->next;
				continue;
			}

			BlenderBone *bone= 0;
			BoneList::iterator it=_bones.find(pchan->name);
			if(it==_bones.end()){
				//something wrong
				pchan=pchan->next;
				continue;
			}
			bone=it->second;

			/// We now need to calculate the pose & channel matrix
			Ogre::Matrix4 chanmat=Ogre::Matrix4::IDENTITY;

			achan= findActionChannel(pchan->name, action);
			if (achan && achan->ipo){
				ipo= achan->ipo;
				Ogre::Vector3 loc= Ogre::Vector3::ZERO;
				Ogre::Vector3 scale= Ogre::Vector3::UNIT_SCALE;
				Ogre::Quaternion rot= Ogre::Quaternion::IDENTITY;

				icu= static_cast<Blender::IpoCurve*>(ipo->curve.first);
				while (icu){
					Ogre::Real curval= 0.f;
					if (icu->bezt){
						curval= BlenderLoadUtil::evaluateIpoCurve(icu, time);
					}

					switch (icu->adrcode){
					case AC_QUAT_W: { rot.w= curval;  break; }
					case AC_QUAT_X: { rot.x= curval;  break; }
					case AC_QUAT_Y: { rot.y= curval;  break; }
					case AC_QUAT_Z: { rot.z= curval;  break; }
					case AC_LOC_X:  { loc.x= curval;  break; }
					case AC_LOC_Y:  { loc.y= curval;  break; }
					case AC_LOC_Z:  { loc.z= curval;  break; }
					case AC_SIZE_X: { scale.x= curval;  break; }
					case AC_SIZE_Y: { scale.y= curval;  break; }
					case AC_SIZE_Z: { scale.z= curval;  break; }
					}
					icu= icu->next;
				}

				/// combine curve results
				MathUtil::makeLocQuatScale(chanmat, loc, rot, scale);
			}

			// update posemat for the new chanmat
			Blender::Bone *bbone= bone->bBone;
			Blender::Bone *bpbone= bone->bPrevBone;

			if (bone->parent){
				Ogre::Matrix4 offs= MathUtil::getFromFloat3(bbone->bone_mat);

				/// NOTE: leaving out bone->head && bpbone->length
				/// in offs (Ogre does not need it)
				// Vector3 len= Vector3((float*)&bbone->head.x);
				// len.y += bpbone->length;
				// offs.setTrans(len);

				// Todo ==> bbone->flag & BONE_HINGE;
				// Todo ==> bbone->flag & BONE_NO_SCALE

				bone->poseMat= (bone->parent->poseMat * offs) * chanmat;
			}else{
				/// use the armature matrix
				Ogre::Matrix4 armamat= MathUtil::getFromFloat(bbone->arm_mat);
				bone->poseMat= armamat * chanmat;
			}
			pchan= pchan->next;
		}
	}


	Blender::bActionChannel *BlenderSkeletonLoadImpl::findActionChannel(const char *name, Blender::bAction *action)
	{
		Blender::bActionChannel *achan= static_cast<Blender::bActionChannel*>(action->chanbase.first);
		while (achan){
			if (*name == achan->name[0] && strcmp(name, achan->name)==0){
				return achan;
			}
			achan= achan->next;
		}
		return NULL;
	}

	//// recurse to build skeleton bone hierachy tree
	void BlenderSkeletonLoadImpl::buildBoneTree(Blender::Bone *current, Blender::Bone *prev,BlenderBone* parent)
	{
		BlenderBone* bone=new BlenderBone(current->name,_bones.size(),current,prev);
		_bones.insert(BoneList::value_type(bone->getName(),bone));

		// matrix is relative to parent bone
		Ogre::Matrix4 parentMat=Ogre::Matrix4::IDENTITY;
		if(prev!=0&&parent!=0){
			BoneList::iterator it=_bones.find(prev->name);
			if(it==_bones.end()){
				OGRE_EXCEPT(Ogre::Exception::ERR_INTERNAL_ERROR,"Can't fine bone parent","BlenderSkeletonLoadImpl::buildBoneTree");
			}
			assert(it->second==parent);
			bone->parent=parent;
			parentMat=parent->parentMat;
		}

		//NOTE:????How these matrix used?
		Ogre::Matrix4 boneMat= MathUtil::getFromFloat(current->arm_mat);
		boneMat=_transform*boneMat;
		bone->parentMat=boneMat.inverse();
		boneMat=parentMat*boneMat;
		bone->boneMat=boneMat;

		/// create the ogre bone
		bone->bone=_skeleton->createBone(bone->getName(),bone->getHandle());
		if(parent){
			assert(parent->bone);
			parent->bone->addChild(bone->bone);
		}
		/// save this transform
		bone->bone->setPosition(boneMat.getTrans());
		Ogre::Quaternion orient=boneMat.extractQuaternion().normalise();
		bone->bone->setOrientation(orient);
		bone->bone->setScale(Ogre::Vector3::UNIT_SCALE);
		bone->bone->setInitialState();
		
		Blender::Bone *child= static_cast<Blender::Bone*>(current->childbase.first);
		while(child){
			buildBoneTree(child, current, bone);
			child= child->next;
		}
	}
}
